import numpy as np
import cv2
import math

def xx(x):
	if x > 0:
		return x - 180
	else:
		return x + 180

def angle(img,rects,landmarks):

	rets = []

	for i,rect in enumerate(rects):
		landmark = landmarks[i]
		size = img.shape

		image_points = np.array([(landmark[30].x,landmark[30].y),     # Nose tip
									(landmark[8].x,landmark[8].y),     # Chin
									(landmark[36].x,landmark[36].y),     # Left eye left corner
									(landmark[45].x,landmark[45].y),     # Right eye right corne
									(landmark[48].x,landmark[48].y),     # Left Mouth corner
									(landmark[54].x,landmark[54].y)      # Right mouth corner
								], dtype="double")

		# 3D model points.
		model_points = np.array([(0.0, 0.0, 0.0),             # Nose tip
									(0.0, -330.0, -65.0),        # Chin
									(-225.0, 170.0, -135.0),     # Left eye left corner
									(225.0, 170.0, -135.0),      # Right eye right corne
									(-150.0, -150.0, -125.0),    # Left Mouth corner
									(150.0, -150.0, -125.0)      # Right mouth corner
								])

		# Camera internals
		focal_length = size[1]
		center = (size[1] / 2, size[0] / 2)
		camera_matrix = np.array([[focal_length, 0, center[0]],
								 [0, focal_length, center[1]],
								 [0, 0, 1]], dtype = "double")

		dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
		(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs)
		#print( "Camera Matrix :\n {0}".format(camera_matrix))
		#print( "Rotation Vector:\n {0}".format(rotation_vector))
		#print( "Translation Vector:\n {0}".format(translation_vector))

		# x down +
		# y clockwise +
		# z right +
		rets.append({
			'x':xx(math.degrees(rotation_vector[0])),
			'y':math.degrees(rotation_vector[1]),
			'z':math.degrees(rotation_vector[2])
			})
	
	return rets
